package roboSoccer;

import java.io.IOException;

import javax.swing.JTextField;

import lejos.pc.comm.*;
import lejos.nxt.remote.NXTCommand;
import lejos.nxt.remote.NXTProtocol;

/**
 * Implements Control in order to communicate and control Lego NXT robots.
 * 
 * @author Ben Hopkins
 * 
 */
public class ControlNXT extends Control implements NXTProtocol {
	
	// variables used for connecting.
	private NXTCommand nxtCommand; // This is actually used to send commands
	// once connected.
	private NXTComm nxtComm;
	private NXTConnector conn;
	private JTextField nameText;

	/**
	 * The constructor attempts to connect with a certain NXT Robot. A
	 * message will print if this is successful or not and the variable
	 * functioning will be set accordingly.
	 * 
	 * @param numRobot The index of the robot connected to bluetooth that it will connect to.
	 */
	public ControlNXT(int numRobot) {
		// Connect
		conn = new NXTConnector();
		nameText = new JTextField(8);
		boolean open = false;
		try {
			nxtCommand = new NXTCommand();
			NXTInfo[] nxti;
			nxti = conn.search(nameText.getText(), null, NXTCommFactory.USB
					| NXTCommFactory.BLUETOOTH);
			// don't try to connect to robots that don't exist
			if(numRobot >= nxti.length)
			{
				System.out.println("Not enough NXTs found on bluetooth.");
				functioning = false;
				return;				
			}
			nxti[numRobot].connectionState = NXTConnectionState.DISCONNECTED;
			nxtComm = NXTCommFactory.createNXTComm(nxti[numRobot].protocol);
			open = nxtComm.open(nxti[numRobot], NXTComm.LCP);
			nxtCommand.setNXTComm(nxtComm);
		} catch (NXTCommException e) {
		}
		if (open) {
			System.out.println("Connection opened.");
			functioning = true;
		} else {
			System.out.println("Connection failed.");
			functioning = false;
		}
	}

	/**
	 * Command to move forwards at a given speed.
	 */
	public void forward(int speed) {
		try {
			nxtCommand.setOutputState(1, (byte) speed, 0, 0, 0, 0, 100);
			nxtCommand.setOutputState(2, (byte) speed, 0, 0, 0, 0, 100);
		} catch (IOException ioe) {
			System.out.println("IOException updating control");
		}
	}

	/**
	 * Rotate command. Negative speed is left and positive speed is right.
	 */
	public void rotate(int speed) {
		try {
			nxtCommand.setOutputState(1, (byte) speed, 0, 0, 0, 0, 100);
			nxtCommand.setOutputState(2, (byte) -speed, 0, 0, 0, 0, 100);
		} catch (IOException ioe) {
			System.out.println("IOException updating control");
		}
	}
	
	/**
	 * Commands a robot to go to a location on the field.
	 * 
	 * @param game Current physical state of the game.
	 * @param r The robot which will take the command.
	 * @param location Vector for the location of the object on the field.
	 * @param turnSpeed Speed with which to turn.
	 * @param driveSpeed Speed with which to drive.
	 * @param leniency How close the robot's angle should be to the ideal calculated angle before moving forwards.
	 * @param targetR The radius of the object the robot is driving towards.
	 * @return Returns true if the robot reaches the radius given in the position of the object.
	 */
	public boolean goToLocation(GameState game, RoboPlayer r,
			Vector location, int turnSpeed, int driveSpeed, int leniency,
			int targetR) {
			
		// are there obstacles? If so correct location accordingly
		for(int j = 0; j < game.teams.length; j++)
			for(int i = 0; i < game.teams[j].players.length; i++)
			{
				RoboPlayer current = game.teams[j].players[i];
				if(r != current)
				{
					Line path = new Line(r.position, location);
					float distance = path.distanceFromCircle(current.position,
							RoboPlayer.sizeRadius*2);
					if(distance < RoboPlayer.passByLength);
					{
						Vector opposite = path.b.oppositeVector();
						opposite.truncate(RoboPlayer.sizeRadius*3);
						Vector locationFromObstacle = Vector.add(current.position, opposite);
						location = locationFromObstacle;
					}
					
				}
			}
		
		Vector angleToLocation = new Vector(0, 0, r.position.vectorAngle(location));
		
		if (angleToLocation.directionAngleDifference(r.position) > leniency)
			rotate(-turnSpeed);
		else if (angleToLocation.directionAngleDifference(r.position) < leniency
				* -1)
			rotate(turnSpeed);
		else
			forward(driveSpeed);

		if (r.position.distance(location) < targetR)
			return true;
		return false;
	}
	
	/**
	 * Commands a robot to turn towards a location on the field.
	 * 
	 * @param game Current physical state of the game.
	 * @param r The robot which will take the command.
	 * @param location Vector for the location of the object to turn to.
	 * @param turnSpeed Speed with which to turn.
	 * @param leniency How close the robot's angle should be to the ideal calculated angle.
	 * @return Returns true if the robot turns in the direction of the location.
	 */
	public boolean turnTowardsLocation(GameState game, RoboPlayer r,
			Vector location, int turnSpeed, int leniency) {
		Vector angleToLocation = new Vector(0, 0, r.position.vectorAngle(location));
		if (angleToLocation.directionAngleDifference(r.position) > leniency)
			rotate(-turnSpeed);
		else if (angleToLocation.directionAngleDifference(r.position) < leniency
				* -1)
			rotate(turnSpeed);
		else
			return true;
		return false;
	}
	
	/**
	 * Commands a robot to turn to an angle
	 * 
	 * @param game Current physical state of the game.
	 * @param r The robot which will take the command.
	 * @param direction A vector whose direction is the direction to turn to. 
	 * 					NOT THE DIRECTION OF THE VECTOR.
	 * @param turnSpeed Speed with which to turn.
	 * @param leniency How close the robot's angle should be to the ideal calculated angle.
	 * @return Returns if the direction has been turned to within leniency.
	 */
	public boolean turnToDirection(GameState game, RoboPlayer r,
			Vector direction, int turnSpeed, int leniency) {
		if (r.position.directionAngleDifference(direction) > leniency)
			rotate(-turnSpeed);
		else if (r.position.directionAngleDifference(direction) < leniency
				* -1)
			rotate(turnSpeed);
		else
			return true;
		return false;
	}

	@Override
	public void stop() {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void kick(int power) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void dribble() {
		// TODO Auto-generated method stub
		
	}

}
